//
// Created by hazyparker on 2022/1/7.
// run /turtle_command service, type defined as std_srvs/Trigger

#include <ros/ros.h>
#include <geometry_msgs//Twist.h>
#include <std_srvs/Trigger.h>

ros::Publisher turtle_vel_pub;
bool pubCommand = false;

bool commandCallback(std_srvs::Trigger::Request &req,
                     std_srvs::Trigger::Response &res){
    // use as flag
    pubCommand = !pubCommand;

    // show request data
    ROS_INFO("publish turtle velocity command [%s]", pubCommand == true? "yes":"no");

    // set feedback data
    res.success = true;
    res.message = "Changed turtle command state...";

    return true;
}

int main(int argc, char **argv){
    // ros node init
    ros::init(argc, argv, "turtle_command_server");

    // create ros handle
    ros::NodeHandle n;

    // create a server named /turtle_command
    // define callback function "commandCallback"
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);

    // create a publisher
    // publish topic named /turtle1/cmd_vel
    // message type defined as geometry_msgs::Twist
    turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

    // loop, waiting for callback function
    ROS_INFO("ready to receive turtle command");
    ros::Rate loop_rate(10);

    while(ros::ok()){
        // check callback queue for new message
        ros::spinOnce();

        // if pubCommand = true
        if(pubCommand){
            geometry_msgs::Twist vel_msg;
            vel_msg.linear.x = 0.5;
            vel_msg.angular.z = 0.2;
            turtle_vel_pub.publish(vel_msg);
        }

        // set loop frequency
        loop_rate.sleep();
    }

    return 0;
}
